#include "M451Series.h"
#include "spi_hal.h"
#include "24l01.h"


short Axis1 = 0;
short Axis2 = 0;
short Axis3 = 0;
short Axis4 = 0;
short Axis5 = 0;
short Axis6 = 0;

void GPIO_Direction_Init(){
	GPIO_SetMode(PB,BIT4,GPIO_MODE_OUTPUT);    //white   the grabber
	GPIO_SetMode(PB,BIT8,GPIO_MODE_OUTPUT);    // yellow  port2
	GPIO_SetMode(PB,BIT9,GPIO_MODE_OUTPUT);    // grey  port1
	GPIO_SetMode(PB,BIT11,GPIO_MODE_OUTPUT);   //green  port4
	
	GPIO_SetMode(PE,BIT5,GPIO_MODE_OUTPUT);  // zise  port5
	GPIO_SetMode(PA,BIT9,GPIO_MODE_OUTPUT);  // blue  port3
	
	PB4 = 0;
	PB8 = 0;
	PB9 = 0;
	PB11 = 0;

	PE5 = 0;
	PA9 = 0;
}
void NRF24L01Init(){
	
	
    CLK_EnableModuleClock(SPI0_MODULE);

    SYS_ResetModule(SPI0_RST);
	
  /* PWM clock frequency can be set equal or double to HCLK by choosing case 1 or case 2 */
    /* case 1.PWM clock frequency is set equal to HCLK: select PWM module clock source as PCLK */
    CLK_SetModuleClock(SPI0_MODULE, CLK_CLKSEL2_PWM0SEL_PCLK0, NULL);
	
    /* Set PB multi-function pins for spi */
    SYS->GPB_MFPL &= ~(SYS_GPB_MFPL_PB5MFP_Msk | SYS_GPB_MFPL_PB6MFP_Msk | SYS_GPB_MFPL_PB7MFP_Msk);
    SYS->GPB_MFPL |= (SYS_GPB_MFPL_PB5MFP_SPI0_MOSI0 | SYS_GPB_MFPL_PB6MFP_SPI0_MISO0|SYS_GPB_MFPL_PB7MFP_SPI0_CLK);
	
    SYS->GPB_MFPH &= ~(SYS_GPB_MFPH_PB13MFP_Msk | SYS_GPB_MFPH_PB14MFP_Msk | SYS_GPB_MFPH_PB15MFP_Msk);
		SYS->GPB_MFPL |= (SYS_GPB_MFPH_PB13MFP_GPIO | SYS_GPB_MFPH_PB14MFP_GPIO|SYS_GPB_MFPH_PB15MFP_GPIO);

}

void PWMInit (){
	
    CLK_EnableModuleClock(PWM0_MODULE);

    SYS_ResetModule(PWM0_RST);
	
    CLK_EnableModuleClock(PWM1_MODULE);

    SYS_ResetModule(PWM1_RST);
  /* PWM clock frequency can be set equal or double to HCLK by choosing case 1 or case 2 */
    /* case 1.PWM clock frequency is set equal to HCLK: select PWM module clock source as PCLK */
    CLK_SetModuleClock(PWM0_MODULE, CLK_CLKSEL2_PWM0SEL_PCLK0, NULL);
	  CLK_SetModuleClock(PWM1_MODULE, CLK_CLKSEL2_PWM1SEL_PCLK1, NULL);

    /* Set PC multi-function pins for PWM0 Channel0~3 */
    SYS->GPC_MFPH = (SYS->GPC_MFPH & (~SYS_GPC_MFPH_PC9MFP_Msk));
    SYS->GPC_MFPH |= SYS_GPC_MFPH_PC9MFP_PWM1_CH0;

    SYS->GPC_MFPH = (SYS->GPC_MFPH & (~SYS_GPC_MFPH_PC10MFP_Msk));
    SYS->GPC_MFPH |= SYS_GPC_MFPH_PC10MFP_PWM1_CH1;
		
    SYS->GPC_MFPL = (SYS->GPC_MFPL & (~SYS_GPC_MFPL_PC0MFP_Msk));
    SYS->GPC_MFPL |= SYS_GPC_MFPL_PC0MFP_PWM0_CH0;
    SYS->GPC_MFPL = (SYS->GPC_MFPL & (~SYS_GPC_MFPL_PC1MFP_Msk));
    SYS->GPC_MFPL |= SYS_GPC_MFPL_PC1MFP_PWM0_CH1;

    SYS->GPC_MFPL = (SYS->GPC_MFPL & (~SYS_GPC_MFPL_PC3MFP_Msk));
    SYS->GPC_MFPL |= SYS_GPC_MFPL_PC3MFP_PWM0_CH3;
    SYS->GPC_MFPL = (SYS->GPC_MFPL & (~SYS_GPC_MFPL_PC4MFP_Msk));
    SYS->GPC_MFPL |= SYS_GPC_MFPL_PC4MFP_PWM0_CH4;
	
    PWM_ConfigOutputChannel(PWM0, 0, 50, 3);  //port 5

		PWM_ConfigOutputChannel(PWM0, 1,100, 50);   // port 4

		PWM_ConfigOutputChannel(PWM0, 3,100, 50);   	//port 6 
		PWM_ConfigOutputChannel(PWM0, 4, 100, 50);   // port 5  the servo motor

    PWM_ConfigOutputChannel(PWM1, 0, 100, 50);   //port 1

		PWM_ConfigOutputChannel(PWM1, 1, 400, 50);  //port 2

    // Enable output of PWM0 channel 0~3
    PWM_EnableOutput(PWM0, 0xF);
    // Enable PWM0 channel 0 period interrupt, use channel 0 to measure time.
    PWM_EnablePeriodInt(PWM0, 0, 0);
    NVIC_EnableIRQ(PWM0P0_IRQn);
		
    // Start
    PWM_Start(PWM1, 0x3);	
    PWM_EnablePeriodInt(PWM1, 0, 0);
    NVIC_EnableIRQ(PWM1P0_IRQn);
    // Start
    PWM_Start(PWM0, 0x1F);			
    PWM_Start(PWM1, 0x1F);			
}


void delay_1s(){
	for(volatile unsigned int x = 0;x < 100; x++){
		for(volatile unsigned int z = 0; z <100; z++){
			z = z;
		}
	}
}

int ParsePackage(unsigned char *dat){
	if( (dat[0] == 0xAA) && (dat[1] == 0xAF) && (dat[2] == 0x03) && (dat[3] == 0x1b)){
		Axis1 = (*(dat + 4)*256 + (*(dat + 5)));
		Axis2 = (*(dat + 6)*256 + (*(dat + 7)));
		Axis3 = (*(dat + 8)*256 + (*(dat + 9)));
		Axis4 = (*(dat + 10)*256 + (*(dat + 11)));
		Axis5 = (*(dat + 12)*256 + (*(dat + 13)));
		Axis6 = (*(dat + 14)*256 + (*(dat + 15)));
	}
}

int main(){
	unsigned char recv[32] = {0};
	GPIO_SetMode(PC,BIT9,GPIO_MODE_INPUT);	    //IRQ
	GPIO_Direction_Init();
	Spi_init();
	RX_Mode();
	PWMInit ();
	while(1){
		if(NRF24L01_RxPacket(recv) == 0) {
			ParsePackage(recv);
		}
	}
}
